package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.*;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;

public class Vision {

    public AxisCamera camera; //Declaration for the camera.
    private ColorImage startImg;
    CriteriaCollection cc;      // the criteria for doing the particle filter operation
    private Debug debug;

    private ColorImage imageConv;
    private BinaryImage thresholdImageConv;
    private BinaryImage bigObjectsImageConv;
    private BinaryImage convexHullImageConv;

    public Vision() {
        //Constructor for the Vision Module, in a Try loop as not to spam the
        //...console with errors anymore.
        //try {
        //Initialize the camera
        this.debug = new Debug();
        this.camera = AxisCamera.getInstance();
        this.camera.writeResolution(AxisCamera.ResolutionT.k320x240);
        this.camera.writeBrightness(50);
        this.camera.writeCompression(10);
        this.cc = new CriteriaCollection();
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
        //}
        //catch(Exception e) {
        //System.out.println("Error initializing Camera: " + e);
        //}
    }

    public ColorImage getThatImage() {
        try {
            return this.camera.getImage();
        } catch (AxisCameraException error) {
            System.out.println("BLAW: " + error);
            return null;
        } catch (NIVisionException error) {
            System.out.println("BLAW: " + error);
            return null;
        }
    }

    public double[][] trackTarget() {
        try {
            double[] targets_x;
            double[] targets_y;
            double[][] target_pairs = new double[2][];
            ColorImage image = camera.getImage(); //Get the image from the Axis Cam
            BinaryImage thresholdImage = image.thresholdRGB(25, 255, 0, 45, 0, 47);   // keep only red objects
            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results
            targets_x = new double[reports.length];
            targets_y = new double[reports.length];
            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                System.out.println("Particle " + i + ":  Center of mass x: " + r.center_mass_x);
                System.out.println("Particle " + i + ":  Center of mass y: " + r.center_mass_y);
                targets_x[i] = r.center_mass_x;
                targets_y[i] = r.center_mass_y;
            }
            System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());

            //Free all the image memory
            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();

            //Prepare to return
            target_pairs[0] = targets_x;
            target_pairs[1] = targets_y;
            return target_pairs;
        } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
            ex.printStackTrace();
        } catch (NIVisionException ex) {
            ex.printStackTrace();
        }
        return null;
    }

    public int onTarget() {
        ParticleAnalysisReport[] reports = null;
        Target[] targets = null;
        ParticleAnalysisReport max = null;
        double lastPerc = 0;
        try {
            ColorImage image = camera.getImage(); //Get the image from the Axis Cam
            BinaryImage thresholdImage = image.thresholdRGB(25, 255, 0, 45, 0, 47);   // keep only red objects
            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results

            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();
        } catch (AxisCameraException axisCameraException) {
        } catch (NIVisionException nIVisionException) {
        }

        targets = new Target[reports.length];

        for(int i = 0; i < reports.length; i++) {
            ParticleAnalysisReport r = reports[i];
            if(r.particleToImagePercent > lastPerc) {
                max = r;
            }
        }

        if(max != null) {
            if(max.center_mass_x_normalized > 0.1) {
                return 1;
            } else if(max.center_mass_x_normalized < -0.1) {
                return -1;
            } else {
                return 0;
            }
        } else {
            return -99;
        }
    }

    public void trackTargetX() {
        try {
            ColorImage image = camera.getImage(); //Get the image from the Axis Cam
            BinaryImage thresholdImage = image.thresholdRGB(25, 255, 0, 45, 0, 47);   // keep only red objects
            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            convexHullImage.write("debugImage.png");
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results
            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                debug.writeToScreen("Prt " + i + "- x: " + r.center_mass_x + " | y: " + r.center_mass_y);
            }
            debug.writeToScreen("Num P: " + filteredImage.getNumberParticles(),2);

            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();
        } catch (AxisCameraException ex) {
            ex.printStackTrace();
            System.out.println("Axis Camera Ex");
        } catch (NIVisionException ex) {
            ex.printStackTrace();
            System.out.println("NI Exception");
        }
    }

    public void getConvex() {
        try {
            if (camera != null) {
                imageConv = camera.getImage();
                if (imageConv != null) {
                    thresholdImageConv = imageConv.thresholdHSV(136, 182, 45, 255, 116, 255);
                    if (thresholdImageConv != null) {
                        convexHullImageConv = thresholdImageConv.convexHull(false);
                        if (convexHullImageConv != null) {
                            convexHullImageConv.write("convex.png");
                            imageConv.free();
                            thresholdImageConv.free();
                            convexHullImageConv.free();
                        } else {
                            System.out.println("ERROR : No convexHullImage!");
                        }
                    } else {
                        System.out.println("ERROR : No thresholdImage!");
                    }
                } else {
                    System.out.println("ERROR : No Image saved!");
                }
            } else {
                System.out.println("ERROR : No Camera!");
            }
        } catch (AxisCameraException ex) {
            ex.printStackTrace();
            System.out.println("Axis Camera Ex: " + ex);
        } catch (NIVisionException ex) {
            ex.printStackTrace();
            System.out.println("NI Exception: " + ex);
        } catch (NullPointerException ex) {
            ex.printStackTrace();
            System.out.println("Null Pointer: " + ex);
        }
    }
}